﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using System.Diagnostics;
using PloobsEngine.Behaviors;
using PloobsEngine.SceneControl;

namespace PloobsEngine.IA
{
    public class DirectionActuator : IActuator
    {
        public DirectionActuator(IObject character)
        {
            this.obj = character;
            this.Name = "DirectionActuator";
            resp.ActuatorName = Name;
            
        }

        private IObject obj;        
        private Vector3 dir;
        private float distanceError = 10;
        ActuatorSensorInformation resp = new ActuatorSensorInformation();
        private bool b = false;

        public float DistanceError
        {
            get { return distanceError; }
            set { distanceError = value; }
        }

        public void SetDirection(Vector3 dir)
        {            
            this.dir = dir;
            this.dir.Normalize();
        }

        private Vector2 ToVector2(Vector3 vec)
        {
            return new Vector2(vec.X, vec.Z);
        }

        public override ActuatorSensorInformation PerformAct()
        {
            ///Checar se ta alinhado e alinhar
            ///            
            float angle = Math.Abs((float) Math.Acos(Vector3.Dot(obj.PhysicObject.FaceVector,dir))) ;

            if(angle > MathHelper.ToRadians(4) && b == true)
            {
                float a = MathHelper.ToDegrees(angle);

                Vector3 aux1 = Vector3.Transform(obj.PhysicObject.FaceVector, Matrix.CreateRotationY(angle));
                aux1.Normalize();
                Vector3 aux2 = Vector3.Transform(obj.PhysicObject.FaceVector, Matrix.CreateRotationY(-angle));
                aux2.Normalize();
                dir.Normalize();

                double d1 = Vector3.Dot(aux1, dir);
                double p = Math.Round(d1, 4);                                
                double a1 = Math.Acos(p);
                float g1 = MathHelper.ToDegrees(Convert.ToSingle( a1));

                double d2 = Vector3.Dot(aux2, dir);                
                double p2 = Math.Round(d2, 4);
                double a2 = Math.Acos(p2);
                float g2 = MathHelper.ToDegrees(Convert.ToSingle(a2));
                
                
                if (a1 > a2)
                {
                    Debug.WriteLine(" > " + a);
                    obj.Behavior.StartBehavior(CharacterMovimentationBehavior.MOVERIGHT, CharacterMovimentationBehavior.MOVERIGHT, null);
                }
                else
                {
                    Debug.WriteLine(" <= " +  a);
                    obj.Behavior.StartBehavior(CharacterMovimentationBehavior.MOVELEFT, CharacterMovimentationBehavior.MOVELEFT, null);
                }
                b = false;
            }
            ///Seguir direcao
            else
            {
                obj.Behavior.StartBehavior(CharacterMovimentationBehavior.MOVEFRONT, CharacterMovimentationBehavior.MOVEFRONT, null);
                b = true;
            }

            resp.Ended = true;
            return resp;

            //DebugVectorDraw.ClearVectorList();
            //DebugVectorDraw.addVector(start - Vector3.Up * 10, Vector3.Up * 50, Color.Yellow);
            //DebugVectorDraw.addVector(end - Vector3.Up * 10, Vector3.Up * 50, Color.YellowGreen);

            
            
        }
    }
}
